/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.team696;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class MainCode extends IterativeRobot {

    DriverStationEnhancedIO cypress = DriverStation.getInstance().getEnhancedIO();
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    PIDController flyWheelController = new PIDController(5.0, 0.07, 1.0);
    
    RobotDrive drive = new RobotDrive(8, 4, 1, 3); //motors
    Talon flywheelMotorTalon1 = new Talon(5);
    Talon flywheelMotorTalon2 = new Talon(2);
    Talon ConveyorTalon = new Talon(6);
    Talon kickUpTalon = new Talon(9);
    Talon pickUpRollersTalon = new Talon(7);
    SmartDashboard dash = new SmartDashboard();
    DigitalInput encoderA = new DigitalInput(2);
    DigitalInput encoderB = new DigitalInput(3);
    Solenoid shooterPos = new Solenoid(3);
    Solenoid frisbeeFeed = new Solenoid(4);
    Solenoid gearBoxClimbLeft = new Solenoid(5);
    Solenoid gearBoxDriveLeft = new Solenoid(6);
    Solenoid gearBoxClimbRight = new Solenoid(7);
    Solenoid gearBoxDriveRight = new Solenoid(8);
    Solenoid firstClimb = new Solenoid(2);
    //Solenoid firstClimb = new Solenoid(2); //wtf here need alot\/
    //Relay secondThirdRelease = new Relay(1);
    Timer flywheelTimer = new Timer();
    int loadPos = 0;                            //to here
    Compressor compressor = new Compressor(1, 2); // compressor
    double leftJoy = 0.0;
    double rightJoy = 0.0;
    double frontRollerSpeed = 0.0;
    double conveyorMotorVal = 0.0;
    double kickUp = 0.0;
    double leftDrive = 0.0;
    double rightDrive = 0.0;
    //PID variables begin here//
    double Kp = 0.0;
    double Ki = 0.0;
    double Kd = 0.0;
    
    double pastTimer = 0.0;
    double pastCount = 0.0;
    double flyWheelSetSpeed = 0.0;
    double flyWheelPower = 0.0;
    double flyWheelSpeed = 0.0;
    //double error = 0.0;
    //double prevError = 0.0;
    //double cumulativeError = 0.0;
    //End PID variables//
    boolean kickUpEnable = false;
    boolean loadEnable = false;
    boolean frontRollers = false;
    boolean shooterEnable = false;
    boolean autoAimEnable = false;
    boolean shooterPosUp = false;
    boolean firstClimbEnable = false;
    boolean secondClimbEnable;
    boolean thirdClimbEnable;
    boolean frisbeeLoad;
    boolean hoodUp = true;
    boolean PTOPosition = false;
    boolean cypressUse = false;
    boolean flyWheelIncreaseOld = false;
    boolean flyWheelIncrease = false;
    boolean flyWheelDecreaseOld = false;
    boolean flyWheelDecrease = false;
    boolean[] oldButtons = new boolean[13];
    boolean[] oldCypressButtons = new boolean[15];
    boolean wantToLoad = false;
    Encoder flyWheelEncoder = new Encoder(4,5, false, CounterBase.EncodingType.k4X); //see if this still works
    Joystick controller = new Joystick(1);
    AnalogChannel distanceSensor = new AnalogChannel(1);
    double leftMotorOld = 0.0;
    double rightMotorOld = 0.0;
    double newTime = 0;
    double oldTime = 0;
    double allowedDistance = 108.0;
    int imageSize = 640;//size of image returned by camera(use for image turn tracking)
    double pastFlywheelCount;
    int autonomousMode = 3;
    double pastFlyweelTimer;

    public void robotInit() {
        drive.tankDrive(0.0, 0.0);
        flywheelMotorTalon1.set(0.0);
        flywheelMotorTalon2.set(0.0);
        //getController();
        getCypress();
        compressor.start();
        flywheelTimer.start();
        flyWheelEncoder.start();
        flywheelMotorTalon2.set(0.0);
        frisbeeFeed.set(false);
        flyWheelEncoder.start();
        //getCypress();
        //firstClimb.set(false);
        //secondThirdRelease.set(Relay.Value.kOff);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {//y
        if (autonomousMode == 1) {
            //non-natural autonomous code.
        } else if (autonomousMode == 2) {
        
        } else if (autonomousMode == 3) {
            if (autoAimEnable) {
                autoAim();
            }
            //only move forward if aimed.
            if ((getDistance() > allowedDistance) && (!autoAimEnable)) {
//                drive.tankDrive((getDistance() - allowedDistance) / 5.0, (getDistance() - allowedDistance) / 5.0);
            }//will need proper PID
            //shoot
            /*if (shooterEnable) {
                flywheelMotorTalon1.set(flywheelMotorTalon1.get() + ((flyWheelSetSpeed - flywheelMotorTalon1.get()) / 2));//replace flyWheelSetSpeed with matlab's speed
            }*///will need proper PID
            //obvious
            //turn around

            //once again, like moving forward, but testing beforehand.
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
//      getCypress();
       Kp = SmartDashboard.getNumber("Kp", 0.0);
       Ki = SmartDashboard.getNumber("Ki", 0.0);
       Kd = SmartDashboard.getNumber("Kd", 0.0); 
       getController();
        
        leftDrive = leftJoy;
        rightDrive =  rightJoy;
//        flyWheelSpeed = (flyWheelEncoder.get() - pastFlywheelCount)/(flywheelTimer.get() - pastFlyweelTimer);
        //pastFlywheelCount = flyWheelEncoder.get();
        //pastFlyweelTimer = flywheelTimer.get();
        //SmartDashboard.putNumber("setPoint", flyWheelSetSpeed);
        //SmartDashboard.putNumber(ERRORS_TO_DRIVERSTATION_PROP, Kp);
        
        //shooter PID functions begin//
        //Kp = dash.getNumber("Kp" , 0.0);
        //Ki = dash.getNumber("Ki", 0.0);
        //Kd = dash.getNumber("Kd", 0.0);
        //flyWheelSpeed = ((flyWheelEncoder.get() - pastCount)/(flywheelTimer.get() - pastTimer)/14000);
        //flyWheelPower = PID(getError(flyWheelSetSpeed, flyWheelSpeed), prevError, Kp, Ki, Kd);
        //flyWheelController.update(flyWheelSetSpeed, flyWheelSpeed);
        //flyWheelPower = flyWheelController.getOutput();
        //dash.putNumber("setPoint", flyWheelSetSpeed);
        //dash.putNumber("position", flyWheelSpeed);
        //dash.putNumber("output", flyWheelPower);
        //pastCount = flyWheelEncoder.get();
        //pastTimer = flywheelTimer.get();
        //shooter PID functions end//
        
        //standard shooter data
        if(shooterEnable){
        flywheelMotorTalon1.set(-flyWheelPower);
        flywheelMotorTalon2.set(-flyWheelPower);
        } else{
            flywheelMotorTalon1.set(0.0);
             flywheelMotorTalon2.set(0.0);   
            }
        
//        if (shooterEnable) {
//            flywheelMotorTalon.set(flywheelMotorTalon.get() + ((flyWheelSetSpeed - flywheelMotorTalon.get()) / 2));
//        } else {
//            if (flywheelMotorTalon.get() > 0.0) {
//                flywheelMotorTalon.set(flywheelMotorTalon.get() + ((0 - flywheelMotorTalon.get()) / 5));
//            } else if (flywheelMotorTalon.get() < 0.0) {
//                flywheelMotorTalon.set(0.0);
//            }
//        }//commented because we're testing on a notbuilt robot
        
        //PTO Data
        gearBoxClimbLeft.set(PTOPosition);
        gearBoxClimbRight.set(PTOPosition); // fix this- move the dependencies to the ptoclimb vars
        gearBoxDriveLeft.set(!PTOPosition);
        gearBoxDriveRight.set(!PTOPosition);
        shooterPos.set(shooterPosUp);
        
        if(PTOPosition){
        leftDrive = Util.constrain(leftDrive, 0.0, 1.0);
        rightDrive = Util.constrain(rightDrive, 0.0, 1.0);
        }
        
        
            kickUpTalon.set(conveyorMotorVal);
            pickUpRollersTalon.set(-conveyorMotorVal);
            ConveyorTalon.set(Util.constrain(conveyorMotorVal, 0, 1));
            
        /*if (cypressUse) {
            pickUpRollersTalon.set(frontRollerSpeed);
            kickUpTalon.set(frontRollerSpeed);
            ConveyorTalon.set(conveyorMotorVal);
        }else {
            pickUpRollersTalon.set(1);
            kickUpTalon.set(-1);
            ConveyorTalon.set(Util.constrain(-frontRollerSpeed,-1,0));
        }*/
                
        
        //driving
        drive.tankDrive(leftDrive, rightDrive);
        
        
        //frisbee loading
        if(frisbeeLoad){//prevent things from happening if pressed with not enough deleay
            frisbeeFeed.set(true);
            loadPos=1;
            wantToLoad = true;
        }
        else if((loadPos>=15)&&wantToLoad){
            frisbeeFeed.set(false);  
            loadPos = 0;
            wantToLoad = false;
        }
        else if(wantToLoad){
            loadPos++;
        }
       
        //climb
       firstClimb.set(firstClimbEnable);
       
       //do we need this? -b
       if (!cypressUse) {
        for(int i = 1; i<13; i++){oldButtons[i] = controller.getRawButton(i);}
       }
       
        System.out.println("Conveyor = "+conveyorMotorVal+" ShootSpeed = "+flyWheelSetSpeed);
       
    }

    /**
     * This function is called periodically during test mode
     */


    public void testPeriodic() {
    }

    /*private double PID(double errorPID, double prevErrorPID, double kProportional, double kIntegral, double kDerivative)
    {
        double output = ((P(errorPID))*kProportional) + (I(errorPID)*kIntegral) + (D(errorPID, prevErrorPID)*kDerivative);
        return(output);
    }
    private double P(double errorP)
    {
        return(errorP);
    }
    private double I(double errorI)
    {
        cumulativeError += errorI;
        return(cumulativeError);
    }
    private double D(double errorD, double prevErrorD)
    {
        errorD = errorD - prevErrorD;
        return(errorD);
    }
    double getError(double setPoint, double position){return(setPoint - position);}
    
*/
    private double getDistance() {//y
        return Util.deadZone((((((102.4) * distanceSensor.getVoltage())) - 36) + ((102.4) * distanceSensor.getVoltage())), (102.4) * distanceSensor.getVoltage() - 0.01, (102.4) * distanceSensor.getVoltage() + 0.01);
    }

    private void autoAim() { /// this is all suspect(Don't You Dare Touch) will most likely be replaced//y
        int xToAim;
        boolean adjustTurn = false;                             //whether or not to turn to see target again.
        double leftMot, rightMot;
        leftMot = 0.1;
        xToAim = (int) SmartDashboard.getNumber("CenterX");
        if (xToAim < 0) {
            xToAim = imageSize;
            adjustTurn = true;
        }
        if (((xToAim - (imageSize / 2) < 0)) ^ (adjustTurn)) {
            leftMot *= -1;
        }
        rightMot = leftMot * -1;
        if (Util.distBetween(xToAim, imageSize / 2) < 10) {
            rightMot = leftMot = 0.0;
            autoAimEnable = false;
        }
        drive.tankDrive(leftMot, rightMot);
    }
    

//    private double wheelSpeed() {//alex is waiting on the encoder
//        pastFlywheelCount = counter.get();
//        pastFlyweelTimer = flywheelTimer.get();
//        return (((counter.get() - pastFlywheelCount) / (flywheelTimer.get() - pastFlyweelTimer)) * 360);
//    }
    public void getController() {
        cypressUse = false;
        leftJoy = controller.getRawAxis(2);
        rightJoy = controller.getRawAxis(4);
        if (controller.getRawButton(2)&&!oldButtons[2]&&(flyWheelSetSpeed>=0.1))
        {
            flyWheelSetSpeed+= 0.1;
        }
        if (controller.getRawButton(3)&&!oldButtons[3]&&(flyWheelSetSpeed<=1))
        {
            flyWheelSetSpeed+=0.1;
        }  
        
        //conveyor: 6 and 8 control speed up/down, 5 controlls overall enable
        if(((controller.getRawButton(6))&&(!oldButtons[6]))&&(conveyorMotorVal>=-1)){
            conveyorMotorVal -= 0.05;                                               //85% power is optimal
        }
        else if(((controller.getRawButton(8)&&(!oldButtons[8])))&&(conveyorMotorVal<=0)){
            conveyorMotorVal += 0.05;
        }
        conveyorMotorVal = (controller.getRawButton(5)) ? conveyorMotorVal : 0;
        
        //front roller speed on off with 5 and 7 (default 0)
        frontRollerSpeed = controller.getRawButton(5) ? 1 : (controller.getRawButton(7) ? -1 : 0);
        
        frisbeeLoad = ((controller.getRawButton(1) && !oldButtons[1]));
        firstClimbEnable = controller.getRawButton(12);
        autoAimEnable = controller.getRawButton(10);
        
        //secondClimbEnable = controller.getRawButton(6);
        //thirdClimbEnable = controller.getRawButton(7);
        
        PTOPosition = controller.getRawButton(9);
        autoAimEnable = controller.getRawButton(10);
        //shooterposition controlled by button 4 - sets to opposite.
        shooterPosUp = (controller.getRawButton(4) && (!oldButtons[4])) ? !shooterPosUp : shooterPosUp;
        //secondClimb = controller.getRawButton(6);
        //thirdClimb = controller.getRawButton(7);
        /*if(controller.getRawButton(9) && !oldButtons[9]){PTOPosition = !PTOPosition;}
        //autoAimEnable = controller.getRawButton(10);
        //shooterEnable = controller.getRawButton(11);
        firstClimbEnable = controller.getRawButton(12);*/
        }


    public void getCypress() {//y
        cypressUse = true;
        try {
            leftJoy = Util.map(cypress.getAnalogIn(7), 0, 3.3, -1, 1); 
       } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            leftJoy = 0;
            System.out.println("Left Joystick error");
        }
        try {
            rightJoy = Util.map(cypress.getAnalogIn(3), 0, 3.3, -1, 1);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            rightJoy = 0;
            System.out.println("Right Joystick error");
        }
        try {
            flyWheelPower = Util.map(cypress.getAnalogIn(2), 0, 3.3, 0.5, 1);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            flyWheelPower = 0;
            System.out.println("Shoot Speed slider error");
        }
        try {
            conveyorMotorVal = Util.rocker(cypress.getAnalogIn(6), 0.5);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            conveyorMotorVal = 0;
            System.out.println("Conveyor rocker error");
        }
        try {
            frontRollerSpeed = Util.rocker(cypress.getAnalogIn(8), 0.5);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            frontRollerSpeed = 0.0;
            System.out.println("frontRollers button Error");
        }
        try {
            shooterEnable = !cypress.getDigital(2);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            shooterEnable = false;
            System.out.println("Shoot button error");
        }
        /*try {
            autoAimEnable = !cypress.getDigital(5);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            autoAimEnable = false;
            System.out.println("autoAim button error");
        }*/
        try {
            shooterPosUp = (cypress.getDigital(8) && (!oldCypressButtons[8])) ? !shooterPosUp : shooterPosUp;
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            shooterPosUp = false;
            System.out.println("shooterPosUp button error");
        }
        try {
            loadEnable = !cypress.getDigital(7);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            loadEnable = false;
            System.out.println("loadEnable error");
        }
        /*try {
            autonomousMode = (int)cypress.getAnalogIn(8);
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            autonomousMode = 3;
            System.out.println("autonomousMode error");
        }*/
        try {
            firstClimbEnable = (cypress.getDigital(4) && (!oldCypressButtons[4])) ? !firstClimbEnable : firstClimbEnable;
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            firstClimbEnable = false;
            System.out.println("ClimbInit Error");
        }
        try {
            PTOPosition = (cypress.getDigital(8) && (!oldCypressButtons[8])) ? !PTOPosition : PTOPosition;
        } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
            PTOPosition = false;
            System.out.println("PTOChange Error");
        }
        try{
        for(int i = 1; i < oldCypressButtons.length + 1; i++){
            oldCypressButtons[i] = cypress.getDigital(i);
        }
        }
        catch(DriverStationEnhancedIO.EnhancedIOException ex){
        for(int i = 1; i < oldCypressButtons.length + 1; i++){
            oldCypressButtons[i] = false;
        }
        }
    }
    
}   
